module gra_forward_2
contains
!////////////////////////////////////////////////////////////////////////////////////
!calculate gravity anomalies by the node density method (mode 2)
!************************************************************************************
subroutine grav_forward_2(no,&
                       &xo,yo,zo,&
                       &nx,ny,nz,&
                       &xmin_mod,xmax_mod,&
                       &ymin_mod,ymax_mod,&
                       &zmin_mod,zmax_mod,&
                       &den,&
                       &dg)
integer no
real    xo(no),yo(no),zo(no),dg(no)
integer nx,ny,nz
real xmin_mod,xmax_mod,ymin_mod,ymax_mod,zmin_mod,zmax_mod
real a(no,(nx+1)*(ny+1)*(nz+1))
real den(1:nx,1:ny,1:nz)
real dden(1:nx+1,1:ny+1,1:nz+1),ddensity((nx+1)*(ny+1)*(nz+1))
!*********************
!calculate node density
!call den2dden(nx,ny,nz,den,dden)
!*********************
!node density vector
call m2v(nx+1,ny+1,nz+1,dden,ddensity)
!*******************
!kernel function
call calc_kernel_2(no,xo,yo,zo,nx,ny,nz,xmin_mod,xmax_mod,ymin_mod,ymax_mod,zmin_mod,zmax_mod,a)
!*******************
! calculate dg
dg=matmul(a,ddensity)
!*******************
end subroutine

!*************************************************************
!calculate node density 
!*************************************************************
subroutine den2dden(nx,ny,nz,den,dden,n1,n2,e1,e2,v1,v2)
real den(1:nx,1:ny,1:nz)
real den0(0:nx+1,0:ny+1,0:nz+1)
real dden(1:nx+1,1:ny+1,1:nz+1)
real den1,den2,den3,den4,den5,den6,den7,den8
integer n1,n2,e1,e2,v1,v2
den0=0.0
den0(1:nx,1:ny,1:nz)=den(1:nx,1:ny,1:nz)
dden=0.0
!$omp parallel do private(j,k,den1,den2,den3,den4,den5,den6,den7,den8)
do i = 1+n1  ,   nx+1-n2
do j = 1+e1  ,   ny+1-e2
do k = 1+v1  ,   nz+1-v2

  den1=den0(i  ,j  ,k-1)
  den2=den0(i  ,j-1,k-1)
  den3=den0(i-1,j-1,k-1)
  den4=den0(i-1,j  ,k-1)
  
  den5=den0(i  ,j  ,k  )
  den6=den0(i  ,j-1,k  )
  den7=den0(i-1,j-1,k  )
  den8=den0(i-1,j  ,k  )
  
  dden(i,j,k)=(den1+den3+den6+den8)-(den2+den4+den5+den7)
enddo
enddo
enddo
!$omp end parallel do
end subroutine
!*************************************************************
!3d density volume to vector
subroutine m2v(nx,ny,nz,den,density)
integer nx,ny,nz
real den(nx,ny,nz)
real density(nx*ny*nz)
!$omp parallel do private(j,k,ijk)
do i=1,nx
do j=1,ny
do k=1,nz
  ijk=(i-1)*(ny)*(nz)+(j-1)*(nz)+k
  density(ijk)=den(i,j,k)
enddo
enddo
enddo
!$omp end parallel do
end subroutine
!*************************************************************
!3d density volume to vector
subroutine v2m(nx,ny,nz,density,den)
integer nx,ny,nz
real den(nx,ny,nz)
real density(nx*ny*nz)
!$omp parallel do private(j,k,ijk)
do i=1,nx
do j=1,ny
do k=1,nz
  ijk=(i-1)*(ny)*(nz)+(j-1)*(nz)+k
  den(i,j,k)=density(ijk)
enddo
enddo
enddo
!$omp end parallel do
end subroutine
!*************************************************************
!kernel function
!*************************************************************
subroutine calc_kernel_2(no,&
                       &xo,yo,zo,&
                       &nx,ny,nz,&
                       &xmin_mod,xmax_mod,&
                       &ymin_mod,ymax_mod,&
                       &zmin_mod,zmax_mod,&
                       &a)
integer no
real xo(no),yo(no),zo(no)
integer nx,ny,nz
real xmin_mod,xmax_mod,ymin_mod,ymax_mod,zmin_mod,zmax_mod
real a(no,(nx+1)*(ny+1)*(nz+1))
real x,y,z
real dym,dxm,dzm

dxm=(xmax_mod-xmin_mod)/(nx)
dym=(ymax_mod-ymin_mod)/(ny)
dzm=(zmax_mod-zmin_mod)/(nz)
!$omp parallel do private(im,jm,km,x,y,z,r,ijk,arg1)
do i=1,no

  do im=1,nx+1
      x=(xmin_mod+(im-1)*dxm)
  do jm=1,ny+1
      y=(ymin_mod+(jm-1)*dym)
  do km=1,nz+1
      z=(zmin_mod+(km-1)*dzm)
    
      ijk=(im-1)*(ny+1)*(nz+1)+(jm-1)*(nz+1)+km
      a(i,ijk)= gnode(xo(i),yo(i),zo(i),x,y,z)
  enddo
  enddo
  enddo
enddo
!$omp end parallel do
end subroutine
!************************************************************************************
real function  gnode(x0,y0,z0,x1,y1,z1)
implicit none
real:: x0,y0,z0,x1,y1,z1!,x2,y2,z2
real gama,twopi
real x,y,z
real s,rijk,ijk
integer i,j,k
real arg1,arg2,arg3

gama=6.67e-3

x = x1-x0
y = y1-y0
z = z1-z0

  rijk=sqrt(x*x+y*y+z*z)

  arg1=atan2((x*y),(z*rijk))
  arg2=alog(rijk+y)
  arg3=alog(rijk+x)

gnode=gama*(z*arg1-x*arg2-y*arg3)

end function

end module
!************************************************************************************
